#include <ros/ros.h>
#include <std_msgs/String.h>
#include <dual_arm_msgs/Plan_State.h>
#include <dual_arm_msgs/MoveJ.h>
#include <dual_arm_msgs/MoveL.h>
#include <dual_arm_msgs/Servo_Move.h>
#include <dual_arm_msgs/MoveJ_P.h>
#include <dual_arm_msgs/Hand_Seq.h>

#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>

using namespace std;

class viGrab
{
private:
    ros::NodeHandle nh;
    ros::Publisher pub_moveL, pub_r_moveJ, pub_l_moveJ, pub_servo_ctr_angle, pub_r_moveJ_P, pub_set_hand_seq;
    ros::Subscriber sub_marker_pose, sub_plan_state;

    //坐标变换Listener
    tf::TransformListener tfListener;

    bool getMarkerPose = false;

    bool grab = false;

    dual_arm_msgs::MoveL msg_moveL_to_target;

public:
    viGrab()
    {
        //机械臂直线规划指令Publisher
        pub_moveL = nh.advertise<dual_arm_msgs::MoveL>("/r_arm/MoveL_Cmd", 10);
        pub_r_moveJ = nh.advertise<dual_arm_msgs::MoveJ>("/r_arm/MoveJ_Cmd", 10);
        pub_l_moveJ = nh.advertise<dual_arm_msgs::MoveJ>("/l_arm/MoveJ_Cmd", 10);
        pub_r_moveJ_P = nh.advertise<dual_arm_msgs::MoveJ_P>("/r_arm/MoveJ_P_Cmd", 10);
        pub_set_hand_seq = nh.advertise<dual_arm_msgs::Hand_Seq>("/r_arm/Hand_SetSeq", 10);
        pub_servo_ctr_angle = nh.advertise<dual_arm_msgs::Servo_Move>("/servo_control/move", 10);

        //视觉识别到marker的Subscriber
        sub_marker_pose = nh.subscribe("/aruco_single/pose", 10, &viGrab::MarkerPoseCallback, this);
        //机械臂规划运行状态的Subscriber
        sub_plan_state = nh.subscribe("/r_arm/Plan_State", 10, &viGrab::PlanStateCallback, this);
    }

    void prepare();
    void PlanStateCallback(const dual_arm_msgs::Plan_State::ConstPtr &msg);
    void MarkerPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &pose_stamped);
};

//抓取前准备：头部舵机转到指定角度位置、左右臂运动到准备状态
void viGrab::prepare()
{
    dual_arm_msgs::Servo_Move servo_move;
    servo_move.servo_id = 1;
    servo_move.angle = 480;
    pub_servo_ctr_angle.publish(servo_move);
    ros::Duration(1.0).sleep();
    servo_move.servo_id = 2;
    servo_move.angle = 532;
    pub_servo_ctr_angle.publish(servo_move);

    dual_arm_msgs::MoveJ l_arm_init;
    l_arm_init.joint[0] = 0;
    l_arm_init.joint[1] = -1.518436;
    l_arm_init.joint[2] = 0;
    l_arm_init.joint[3] = 0;
    l_arm_init.joint[4] = 0;
    l_arm_init.joint[5] = 0;
    l_arm_init.joint[6] = 0;
    l_arm_init.speed = 0.3;
    dual_arm_msgs::MoveJ r_arm_init;
    r_arm_init.joint[0] = 2.757201;
    r_arm_init.joint[1] = -0.483002;
    r_arm_init.joint[2] = -1.570796;
    r_arm_init.joint[3] = -2.045456;
    r_arm_init.joint[4] = -0.414289;
    r_arm_init.joint[5] = 0.784787;
    r_arm_init.joint[6] = -0.337686;

    // r_arm_init.joint[0] = 2.574954;
    // r_arm_init.joint[1] = -0.588106;
    // r_arm_init.joint[2] = -1.570604;
    // r_arm_init.joint[3] = -1.938851;
    // r_arm_init.joint[4] = -0.398284;
    // r_arm_init.joint[5] = 0.930557;
    // r_arm_init.joint[6] = -0.157795;

    // r_arm_init.joint[0] = 2.05797;
    // r_arm_init.joint[1] = -1.076013;
    // r_arm_init.joint[2] = -2.045875;
    // r_arm_init.joint[3] = -2.284741;
    // r_arm_init.joint[4] = -0.180013;
    // r_arm_init.joint[5] = 1.886718;
    // r_arm_init.joint[6] = 0.679841;
    r_arm_init.speed = 0.3;
    pub_r_moveJ.publish(r_arm_init);
    pub_l_moveJ.publish(l_arm_init);

    ros::Duration(1.0).sleep();
    dual_arm_msgs::Hand_Seq msg_set_hand_seq;
    msg_set_hand_seq.seq_num = 1;
    pub_set_hand_seq.publish(msg_set_hand_seq);
    ros::Duration(2.0).sleep();
    getMarkerPose = true;
}

// ros::Publisher pub_r_moveJ;
// bool getMarkerPose = false;

//订阅视觉识别到marker的回调函数
void viGrab::MarkerPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &pose_stamped)
{
    if (getMarkerPose)
    {
        getMarkerPose = false;

        try
        {
            // //坐标变换Listener
            // tf::TransformListener tfListener;
            //获取base_link和camera_color_frame之间的关系，也就是手眼标定的结果
            ROS_INFO("waitForTransform ok [base_link, camera_color_frame]");
            tfListener.waitForTransform("/base_link", "/camera_color_frame",
                                        ros::Time(0), ros::Duration(2.0));

            // marker在相机坐标系下的位姿
            geometry_msgs::PoseStamped marker_pose_in_camera;
            // marker在机械臂base_link坐标系下的位姿
            geometry_msgs::PoseStamped marker_pose_in_base_link;

            marker_pose_in_camera.pose = pose_stamped->pose;
            marker_pose_in_camera.header = pose_stamped->header;
            marker_pose_in_camera.header.stamp = ros::Time();
            marker_pose_in_base_link.header.stamp = ros::Time();

            //将marker在相机坐标系下的位姿转换为在机械臂base_link坐标系下的位姿
            tfListener.transformPose("base_link", marker_pose_in_camera, marker_pose_in_base_link);

            ROS_INFO("MarkerPoseCallback: trans pose->position[%f, %f, %f]\n",
                     marker_pose_in_base_link.pose.position.x, marker_pose_in_base_link.pose.position.y,
                     marker_pose_in_base_link.pose.position.z);
            ROS_INFO("MarkerPoseCallback: trans pose->orientation[%f, %f, %f, %f]\n",
                     marker_pose_in_base_link.pose.orientation.x,
                     marker_pose_in_base_link.pose.orientation.y,
                     marker_pose_in_base_link.pose.orientation.z,
                     marker_pose_in_base_link.pose.orientation.w);

            // MoveJ_P
            // dual_arm_msgs::MoveJ_P msg_moveJ_P;
            // // msg_moveJ_P.Pose.position.x = marker_pose_in_base_link.pose.position.x;
            // // msg_moveJ_P.Pose.position.y = marker_pose_in_base_link.pose.position.y;
            // // msg_moveJ_P.Pose.position.z = marker_pose_in_base_link.pose.position.z;
            // // msg_moveJ_P.Pose.orientation.x = 0.268323;
            // // msg_moveJ_P.Pose.orientation.y = -0.4591506;
            // // msg_moveJ_P.Pose.orientation.z = 0.7376913;
            // // msg_moveJ_P.Pose.orientation.w = 0.4159854;
            // msg_moveJ_P.Pose.position.x = marker_pose_in_base_link.pose.position.y * (-1) - 0.135;
            // msg_moveJ_P.Pose.position.y = marker_pose_in_base_link.pose.position.z * (-1) - 0.03;
            // msg_moveJ_P.Pose.position.z = marker_pose_in_base_link.pose.position.x;
            // msg_moveJ_P.Pose.orientation.x = -0.620366;
            // msg_moveJ_P.Pose.orientation.y = 0.3357773;
            // msg_moveJ_P.Pose.orientation.z = -0.7085193;
            // msg_moveJ_P.Pose.orientation.w = 0.0200002;
            // // msg_moveJ_P.Pose.orientation.x = -0.1966974;
            // // msg_moveJ_P.Pose.orientation.y = 0.6794706;
            // // msg_moveJ_P.Pose.orientation.z = -0.501678;
            // // msg_moveJ_P.Pose.orientation.w = 0.4979448;
            // msg_moveJ_P.speed = 0.2;
            // pub_r_moveJ_P.publish(msg_moveJ_P);

            // MoveL
            msg_moveL_to_target.Pose.position.x = marker_pose_in_base_link.pose.position.y * (-1) - 0.15;
            msg_moveL_to_target.Pose.position.y = marker_pose_in_base_link.pose.position.z * (-1) - 0.01;
            msg_moveL_to_target.Pose.position.z = marker_pose_in_base_link.pose.position.x;
            msg_moveL_to_target.Pose.orientation.x = -0.6882535;
            msg_moveL_to_target.Pose.orientation.y = 0.1694596;
            msg_moveL_to_target.Pose.orientation.z = -0.6974555;
            msg_moveL_to_target.Pose.orientation.w = 0.1055766;

            // msg_moveL_to_target.Pose.orientation.x = -0.620366;
            // msg_moveL_to_target.Pose.orientation.y = 0.3357773;
            // msg_moveL_to_target.Pose.orientation.z = -0.7085193;
            // msg_moveL_to_target.Pose.orientation.w = 0.0200002;
            msg_moveL_to_target.speed = 0.3;

            pub_moveL.publish(msg_moveL_to_target);

            grab = true;
        }
        catch (tf::TransformException &ex)
        {
            ROS_ERROR("[adventure_tf]: (wait) %s", ex.what());
        }
    }
}

//订阅机械臂执行规划返回状态的回调函数
void viGrab::PlanStateCallback(const dual_arm_msgs::Plan_State::ConstPtr &msg)
{
    if (msg->state)
    {
        ROS_INFO("PlanStateCallback: success!\n");
        if (grab)
        {
            grab = false;
            ros::Duration(1.0).sleep();
            dual_arm_msgs::Hand_Seq msg_set_hand_seq;
            msg_set_hand_seq.seq_num = 2;
            pub_set_hand_seq.publish(msg_set_hand_seq);

            ros::Duration(3.5).sleep();
            // dual_arm_msgs::MoveL msg_moveL_z_up;
            // msg_moveL_z_up.Pose.position.x = msg_moveL_to_target.Pose.position.x;
            // msg_moveL_z_up.Pose.position.y = msg_moveL_to_target.Pose.position.y;
            // msg_moveL_z_up.Pose.position.z = msg_moveL_to_target.Pose.position.z + 0.03;
            // msg_moveL_z_up.Pose.orientation = msg_moveL_to_target.orientation;
            // msg_moveL_to_target.speed = 0.3;
            msg_moveL_to_target.Pose.position.z += 0.03;
            pub_moveL.publish(msg_moveL_to_target);
        }
    }
    else
    {
        ROS_INFO("PlanStateCallback: failed!\n");
    }
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "demo_vi_grab");
    // 创建节点句柄
    // ros::NodeHandle nh;

    viGrab viGrab;
    ros::Duration(2.0).sleep();
    viGrab.prepare();

    ros::AsyncSpinner spin(1);
    spin.start();

    ros::waitForShutdown();

    return 0;
}